#include "Matrix.h"
#include "Maths.h"

namespace Maths
{
Matrix::Matrix(Matrix& o)
{
	memcpy(m, o.m, sizeof(m));
}

Matrix::Matrix()
{
	Identity();
}

Matrix::Matrix(float left,float right,float bottom,float top,float zNear,float zFar)
{
	Identity();
	float *m = (float*)(this->m);
	float tx=-(right+left)/(right-left);
	float ty=-(top+bottom)/(top-bottom);
	float tz=-(zFar+zNear)/(zFar-zNear);
	m[0]=2/(right-left);
	m[5]=2/(top-bottom);
	m[10]=-2/(zFar-zNear);
	m[12]=tx;
	m[13]=ty;
	m[14]=tz;
}

Matrix::Matrix(float fovy, float aspect, float zNear,float zFar)
{
	float f = 1.0f/tanf(fovy * ((float)(Maths::PI)/360.0f));
	float *m = (float*)(this->m);

    m[0] = f / aspect;
    m[1] = 0.0;
    m[2] = 0.0;
    m[3] = 0.0;
 
    m[4] = 0.0;
    m[5] = f;
    m[6] = 0.0;
    m[7] = 0.0;
 
    m[8] = 0.0;
    m[9] = 0.0;
    m[10] = (zFar + zNear) / (zNear - zFar);
    m[11] = -1.0f;
 
    m[12] = 0.0;
    m[13] = 0.0;
    m[14] = 2.0f * (zFar * zNear) / (zNear - zFar);
	m[15] = 0.0;
 
}

Matrix& Matrix::operator=(Matrix& o)
{
	memcpy(m, o.m, sizeof(m));
	return *this;
}

void Matrix::Translate( float x, float y, float z )
{
	Matrix t;

	t(0,3) = x;
	t(1,3) = y;
	t(2,3) = z;

	*this *= t;
}

void Matrix::Translate( Vector3D& translation )
{
	Matrix t;
	float &x = translation.X(), &y = translation.Y(), &z = translation.Z();

	t(0,3) = x;
	t(1,3) = y;
	t(2,3) = z;

	*this *= t;
}
void Matrix::RotateX( float angle )
{
	float cosa = cos(angle);
	float sina = sin(angle);

	Matrix r;

	r(1,1) = cosa;
	r(2,1) = sina;
	r(1,2) = -sina;
	r(2,2) = cosa;

	*this *= r;
}
void Matrix::RotateY( float angle )
{
	float cosa = cos(angle);
	float sina = sin(angle);

	Matrix r;

	r(0,0) = cosa;
	r(0,2) = sina;
	r(2,0) = -sina;
	r(2,2) = cosa;

	*this *= r;
}
void Matrix::RotateZ( float angle )
{
	float cosa = cos(angle);
	float sina = sin(angle);
	
	Matrix r;

	r(0,0) = cosa;
	r(0,1) = -sina;
	r(1,0) = sina;
	r(1,1) = cosa;

	*this *= r;
}

void Matrix::Scale( float x, float y, float z )
{
	Matrix s;
	s(0,0) = x;
	s(1,1) = y;
	s(2,2) = z;
	*this *= s;
}

void Matrix::Identity()
{
	memset(m, 0, sizeof(m));
	m[0][0] = 1;
	m[1][1] = 1;
	m[2][2] = 1;
	m[3][3] = 1;
}
void Matrix::Zero()
{
	memset(m, 0, sizeof(m));
}

Matrix& Matrix::operator*=(Matrix& o)
{
	Matrix a3;
	a3.Zero();
	for(int i = 0; i < 4; i++) 
		for(int  j = 0; j < 4; j++)
			for(int k = 0; k < 4; k++) 
				a3(i,j) +=  (*this)(i, k) * o(k, j);
	memcpy(m, a3.m, sizeof(m));
	return *this;
}

Matrix Matrix::operator*(Matrix& o)
{
	Matrix a3 = o;
	a3 *= *this;
	return a3;
}

Vector3D Matrix::operator*(Vector3D& o)
{
	Vector3D result;
	result.X() = o.X()*m[0][0] + o.Y()*m[1][0] + o.Z()*m[2][0] + m[3][0];
	result.Y() = o.X()*m[0][1] + o.Y()*m[1][1] + o.Z()*m[2][1] + m[3][1];
	result.Z() = o.X()*m[0][2] + o.Y()*m[1][2] + o.Z()*m[2][2] + m[3][2];
	return result;
};

Vector3D Matrix::mvMulti(Vector3D& o, float* w)
{
	Vector3D result;
	result.X() = o.X()*m[0][0] + o.Y()*m[1][0] + o.Z()*m[2][0] + m[3][0];
	result.Y() = o.X()*m[0][1] + o.Y()*m[1][1] + o.Z()*m[2][1] + m[3][1];
	result.Z() = o.X()*m[0][2] + o.Y()*m[1][2] + o.Z()*m[2][2] + m[3][2];
	if (w)  *w = o.X()*m[0][3] + o.Y()*m[1][3] + o.Z()*m[2][3] + m[3][3];
	return result;
};

void Matrix::fromHeadPitchRoll(float headDegrees, float pitchDegrees, float rollDegrees)
{
    // Constructs a rotation matrix based on a Euler Transform.
    // I use the popular NASA standard airplane convention of 
    // heading-pitch-roll (i.e., RzRxRy).

    headDegrees = headDegrees*float(Maths::DEGTORAD);
    pitchDegrees = pitchDegrees*float(Maths::DEGTORAD);
    rollDegrees = rollDegrees*float(Maths::DEGTORAD);

    float cosH = cosf(headDegrees);
    float cosP = cosf(pitchDegrees);
    float cosR = cosf(rollDegrees);
    float sinH = sinf(headDegrees);
    float sinP = sinf(pitchDegrees);
    float sinR = sinf(rollDegrees);

    (*this)(0, 0) = cosR * cosH - sinR * sinP * sinH;
    (*this)(0, 1) = sinR * cosH + cosR * sinP * sinH;
    (*this)(0, 2) = -cosP * sinH;
    (*this)(0, 3) = 0.0f;

    (*this)(1, 0) = -sinR * cosP;
    (*this)(1, 1) = cosR * cosP;
    (*this)(1, 2) = sinP;
    (*this)(1, 3) = 0.0f;

    (*this)(2, 0) = cosR * sinH + sinR * sinP * cosH;
    (*this)(2, 1) = sinR * sinH - cosR * sinP * cosH;
    (*this)(2, 2) = cosP * cosH;
    (*this)(2, 3) = 0.0f;

    (*this)(3, 0) = 0.0f;
    (*this)(3, 1) = 0.0f;
    (*this)(3, 2) = 0.0f;
    (*this)(3, 3) = 1.0f;
}

void Matrix::toHeadPitchRoll(float &headDegrees, float &pitchDegrees, float &rollDegrees) const
{
    // Extracts the Euler angles from a rotation matrix. The returned
    // angles are in degrees. This method might suffer from numerical
    // imprecision for ill defined rotation matrices.
    //
    // This function only works for rotation matrices constructed using
    // the popular NASA standard airplane convention of heading-pitch-roll 
    // (i.e., RzRxRy).
    //
    // The algorithm I use here is from a paper written by David Eberly
    // titled "Euler Angle Formulas". This paper can be found on his
    // Magic Software web site (http://www.magic-software.com).

    float thetaX = asinf(m[1][2]);
    float thetaY = 0.0f;
    float thetaZ = 0.0f;

	Matrix* me = const_cast<Matrix*>(this);

    if (thetaX < (Maths::PI/2.0f))
    {
        if (thetaX > -(Maths::PI/2.0f))
        {
            thetaZ = atan2f(-(*me)(1, 0), (*me)(1, 1));
            thetaY = atan2f(-(*me)(0, 2), (*me)(2, 2));
        }
        else
        {
            // Not a unique solution.
            thetaZ = -atan2f((*me)(2, 0), (*me)(0, 0));
            thetaY = 0.0f;
        }
    }
    else
    {
        // Not a unique solution.
        thetaZ = atan2f((*me)(2, 0), (*me)(0, 0));
        thetaY = 0.0f;
    }

    headDegrees = (thetaY*float(Maths::DEGTORAD));
    pitchDegrees = (thetaX*float(Maths::DEGTORAD));
    rollDegrees = (thetaZ*float(Maths::DEGTORAD));
}
bool gluInvertMatrix(const float m[16], float invOut[16])
{
    double inv[16], det;
    int i;

    inv[0] = m[5]  * m[10] * m[15] - 
             m[5]  * m[11] * m[14] - 
             m[9]  * m[6]  * m[15] + 
             m[9]  * m[7]  * m[14] +
             m[13] * m[6]  * m[11] - 
             m[13] * m[7]  * m[10];

    inv[4] = -m[4]  * m[10] * m[15] + 
              m[4]  * m[11] * m[14] + 
              m[8]  * m[6]  * m[15] - 
              m[8]  * m[7]  * m[14] - 
              m[12] * m[6]  * m[11] + 
              m[12] * m[7]  * m[10];

    inv[8] = m[4]  * m[9] * m[15] - 
             m[4]  * m[11] * m[13] - 
             m[8]  * m[5] * m[15] + 
             m[8]  * m[7] * m[13] + 
             m[12] * m[5] * m[11] - 
             m[12] * m[7] * m[9];

    inv[12] = -m[4]  * m[9] * m[14] + 
               m[4]  * m[10] * m[13] +
               m[8]  * m[5] * m[14] - 
               m[8]  * m[6] * m[13] - 
               m[12] * m[5] * m[10] + 
               m[12] * m[6] * m[9];

    inv[1] = -m[1]  * m[10] * m[15] + 
              m[1]  * m[11] * m[14] + 
              m[9]  * m[2] * m[15] - 
              m[9]  * m[3] * m[14] - 
              m[13] * m[2] * m[11] + 
              m[13] * m[3] * m[10];

    inv[5] = m[0]  * m[10] * m[15] - 
             m[0]  * m[11] * m[14] - 
             m[8]  * m[2] * m[15] + 
             m[8]  * m[3] * m[14] + 
             m[12] * m[2] * m[11] - 
             m[12] * m[3] * m[10];

    inv[9] = -m[0]  * m[9] * m[15] + 
              m[0]  * m[11] * m[13] + 
              m[8]  * m[1] * m[15] - 
              m[8]  * m[3] * m[13] - 
              m[12] * m[1] * m[11] + 
              m[12] * m[3] * m[9];

    inv[13] = m[0]  * m[9] * m[14] - 
              m[0]  * m[10] * m[13] - 
              m[8]  * m[1] * m[14] + 
              m[8]  * m[2] * m[13] + 
              m[12] * m[1] * m[10] - 
              m[12] * m[2] * m[9];

    inv[2] = m[1]  * m[6] * m[15] - 
             m[1]  * m[7] * m[14] - 
             m[5]  * m[2] * m[15] + 
             m[5]  * m[3] * m[14] + 
             m[13] * m[2] * m[7] - 
             m[13] * m[3] * m[6];

    inv[6] = -m[0]  * m[6] * m[15] + 
              m[0]  * m[7] * m[14] + 
              m[4]  * m[2] * m[15] - 
              m[4]  * m[3] * m[14] - 
              m[12] * m[2] * m[7] + 
              m[12] * m[3] * m[6];

    inv[10] = m[0]  * m[5] * m[15] - 
              m[0]  * m[7] * m[13] - 
              m[4]  * m[1] * m[15] + 
              m[4]  * m[3] * m[13] + 
              m[12] * m[1] * m[7] - 
              m[12] * m[3] * m[5];

    inv[14] = -m[0]  * m[5] * m[14] + 
               m[0]  * m[6] * m[13] + 
               m[4]  * m[1] * m[14] - 
               m[4]  * m[2] * m[13] - 
               m[12] * m[1] * m[6] + 
               m[12] * m[2] * m[5];

    inv[3] = -m[1] * m[6] * m[11] + 
              m[1] * m[7] * m[10] + 
              m[5] * m[2] * m[11] - 
              m[5] * m[3] * m[10] - 
              m[9] * m[2] * m[7] + 
              m[9] * m[3] * m[6];

    inv[7] = m[0] * m[6] * m[11] - 
             m[0] * m[7] * m[10] - 
             m[4] * m[2] * m[11] + 
             m[4] * m[3] * m[10] + 
             m[8] * m[2] * m[7] - 
             m[8] * m[3] * m[6];

    inv[11] = -m[0] * m[5] * m[11] + 
               m[0] * m[7] * m[9] + 
               m[4] * m[1] * m[11] - 
               m[4] * m[3] * m[9] - 
               m[8] * m[1] * m[7] + 
               m[8] * m[3] * m[5];

    inv[15] = m[0] * m[5] * m[10] - 
              m[0] * m[6] * m[9] - 
              m[4] * m[1] * m[10] + 
              m[4] * m[2] * m[9] + 
              m[8] * m[1] * m[6] - 
              m[8] * m[2] * m[5];

    det = m[0] * inv[0] + m[1] * inv[4] + m[2] * inv[8] + m[3] * inv[12];

    if (det == 0)
        return false;

    det = 1.0 / det;

    for (i = 0; i < 16; i++)
        invOut[i] = inv[i] * det;

    return true;
}

Matrix Matrix::getInvert()
{
	Matrix result;
	gluInvertMatrix((float*)m, (float*)result.m);
	return result;
}
};